#include "util.h"

/* Function prototypes */

void sort_f_helper(data_t *A, int p, int r, data_t * left);
static void merge_f(data_t *A, int p, int q, int r, data_t * left);
static void copy_f(data_t * source, data_t * dest, int n) ;

/* Function definitions */


/* Basic merge sort */
inline void sort_f_helper(data_t *A, int p, int r, data_t * left) 
{
	assert (A) ;
	if (p < r)
	{
		if (r - p < 25) {
			isort(A+p, A+r);
		} else {
			int q = (p + r) / 2 ;
			sort_f_helper(A, p, q, left);
			sort_f_helper(A, q + 1, r, left);
			merge_f(A, p, q, r, left) ;
		}
	}	
}

/*  */
void sort_f(data_t *A, int p, int r) 
{
	assert (A) ;
	assert (p >= 0);
	assert (r >= 0);
	if (p < 0)
		return;
	if (r < 0)
		return;
	if (p < r)
	{
		if (r - p < 25) {
			isort(A+p, A+r);
		} else {
			data_t * left = 0;
			int q = (p + r) / 2;
			mem_alloc(&left, q + 2);
			if (left == NULL) {
				mem_free(&left);
				return;
			}
			
			sort_f_helper(A, p, q, left);
			sort_f_helper(A, q+1, r, left);
			merge_f(A, p, q, r, left);
			
			mem_free(&left);
		}
	}	
}


/* A merge routine. Merges the sub-arrays A [p..q] and A [q + 1..r].
 * Uses two arrays 'left' and 'right' in the merge operation.
 */
inline static void merge_f(data_t * __restrict A, int p, int q, int r, data_t * left) 
{ 
	assert(A) ;
	assert(p <= q) ;
	assert((q + 1) <= r) ;
	int n1 = q - p + 1;
	int n2 = r - q ;
	
	copy_f (&(A [p]), left, n1) ;

	left [n1] = UINT_MAX ;

	data_t * __restrict lptr = left;
	A = A+p;
	data_t * __restrict rptr = A + n1;

	data_t * rend = rptr + n2;
	data_t * lend = lptr + n1;

	
	while ((rptr < rend) && (lptr < lend)) {
		data_t cmp = (*lptr <= *rptr);
		data_t min = *rptr ^ ((*rptr ^ *lptr) & (-cmp));
		*A++ = min;
		lptr += cmp;
		rptr += !cmp;
	}

	while (lptr < lend) {
		*A++ = *lptr++;
	}
	
	while (rptr < rend) {
		*A++ = *rptr++;
	}
}

inline static void copy_f(data_t * source, data_t * dest, int n)
{                                                                             
	assert (dest) ;                                                       
	assert (source) ;                                                     
	data_t * sptr = source;
	data_t * dptr = dest;
	int i ;                                                               
	for (i = 0 ; i < n ; i++) {
		*(dptr + i) = *(sptr + i);
	}                                                                     
}
